/*!
 *
 * INSERT A HEADER HERE
 *
 */

#ifndef ROSPKG_SCHUNK_HIGH_NEEDLE_STEERING_CONTROLLER_H_
#define ROSPKG_SCHUNK_HIGH_NEEDLE_STEERING_CONTROLLER_H_

/******************************************************************************
                                   INCLUDES
 *****************************************************************************/

// Schunk_high
#include <schunk_high/cartesian_controller.h>

// Messages
#include <schunk_msgs/InsertionVelocityDutyCicle.h>

class NeedleSteeringController : public CartesianController
{
    private:

        // Publishers
        ros::Publisher publisher_insertion_velocity_dutycicle_;

        // Subscribers
        ros::Subscriber subscriber_insertion_velocity_dutycicle_references_;

        // Service Servers
        ros::ServiceServer service_server_init_control_;
        ros::ServiceServer service_server_enable_control_;
        ros::ServiceServer service_server_disable_control_;

        // Messages
        schunk_msgs::InsertionVelocityDutyCicle publisher_insertion_velocity_dutycicle_message_;
        schunk_msgs::InsertionVelocityDutyCicle subscriber_insertion_velocity_dutycicle_references_message_;

        // Subscribed Topic Callbacks
        void ReferencesCallback(const schunk_msgs::InsertionVelocityDutyCicle::ConstPtr& msg);

        // Services Server Callbacks
        bool InitCallback(schunk_high::Init::Request& req, schunk_high::Init::Response& resp);
        bool EnableCallback(schunk_high::Enable::Request& req, schunk_high::Enable::Response& resp);
        bool DisableCallback(schunk_high::Disable::Request& req, schunk_high::Disable::Response& resp);

        // Publisher Wrap Functions
        void PublishInsertionVelocityDutyCicle();

        // Internal Functions
        void UpdateReferences();
        void SetSchunkVelocitiesAndSend();

        // Internal Control Variables
        double reference_dutycicle_;
        double reference_rotation_velocity_;
        double insertion_window_size_;

    public:

        // Constructor
        NeedleSteeringController();

        // Destructor
        ~NeedleSteeringController();

        // Control Loop Function
        void Control();
};

#endif  // ROSPKG_SCHUNK_HIGH_NEEDLE_STEERING_CONTROLLER_H_
